package behavior;

import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.subsumption.Behavior;

public class Step implements Behavior {
    private DifferentialPilot pilot = new DifferentialPilot(28f, 105f, Motor.C, Motor.A, false);
	private UltrasonicSensor sonar;
	private boolean suppressed = false;

	public Step(SensorPort port)
	{
		sonar = new UltrasonicSensor(port);
	}

	public boolean takeControl() {
		try {
			Thread.sleep(50);
		} catch (InterruptedException e) {
			e.printStackTrace();
		}

		return ((sonar.getDistance() > 10) && (Motor.B.getTachoCount() == 0));
	}

	public void suppress() {
		suppressed = true;
	}

	public void action() {

		suppressed = false;
		LCD.clear();
		LCD.drawString("Step detected", 0, 2);

		pilot.setTravelSpeed(50);
		pilot.travel(-60, true);
		
		while( pilot.isMoving() && !suppressed )
			Thread.yield();
		
		pilot.stop();
		
		Motor.B.rotateTo(-90, true);

		while( Motor.B.isMoving() && !suppressed )
			Thread.yield();

		Motor.B.stop();

		if(Motor.B.getTachoCount() > -80){
			while(Motor.B.getTachoCount() > -80){
				Motor.B.rotateTo(-90, true);
				while( Motor.B.isMoving() && !suppressed )
					Thread.yield();
			}
			Motor.B.stop();
		}
	}
}
